#!/usr/bin/env python
# BEGIN ALL

import rospy,cv2,cv_bridge, numpy
from sensor_msgs.msg import Image
from std_msgs.msg import Float64
from sensor_msgs.msg import LaserScan
from ackermann_msgs.msg import AckermannDriveStamped
from gazebo_msgs.srv import *
from geometry_msgs.msg import *
import racecar_speed_line
pi = 3.1415926
dt = 0.2

rospy.init_node('follower')

rospy.wait_for_service('/gazebo/set_model_state')
set_state_service = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)

objstate = SetModelStateRequest()

print("-"*20 + "Reset the car" + "-"*20)
objstate = SetModelStateRequest()
objstate.model_state.model_name = "racecar"
objstate.model_state.pose.position.x = -0.3
objstate.model_state.pose.position.y = -0.3
objstate.model_state.pose.position.z = 0
objstate.model_state.pose.orientation.w = 0.707
objstate.model_state.pose.orientation.x = 0
objstate.model_state.pose.orientation.y = 0
objstate.model_state.pose.orientation.z = 0.707
objstate.model_state.twist.linear.x = 0
objstate.model_state.twist.linear.y = 0
objstate.model_state.twist.linear.z = 0
objstate.model_state.twist.angular.x = 0
objstate.model_state.twist.angular.y = 0
objstate.model_state.twist.angular.z = 0
objstate.model_state.reference_frame = "world"
result = set_state_service(objstate)


cmd_vel_pub = rospy.Publisher(
        "/vesc/low_level/ackermann_cmd_mux/input/teleop", AckermannDriveStamped, queue_size=1)

ack = AckermannDriveStamped()

# angle = [0,-pi/32,-pi/16,-pi/8,-pi/4,-pi/8,-pi/3,-pi/3,-pi/8,pi/4,pi/3,pi/3,pi/3,pi/3,-pi/8,-pi/4,-pi/3,-pi/2,-pi/4,pi/8,pi/16,pi/32,0,0,0,0,pi/4,pi/3,pi/4,pi/16,0]
angle = [0,-pi/32,-pi/16,-pi/8,-pi/4,-pi/8,-pi/3,-pi/3,-pi/8,pi/4,pi/3,pi/3,pi/3,pi/3,-pi/8,-pi/4,-pi/3,-pi/2,-pi/4,pi/8,pi/16,0,0,0,0,pi/4,pi/3,pi/4,pi/14]
speed = []


def cla_speed(ang):
	if abs(ang) < pi/16:
		return 1.4
	elif abs(ang) < pi/8:
	    return 1.2
	elif abs(ang) < pi/4:
	    return 1.0
	elif abs(ang) < pi/2:
	    return 0.8
	else:
	    return 0.6
	    

    
    
print("angle",len(angle))
print("speed",len(speed))


for i in range(len(angle)):
    speed = cla_speed(angle[i])
    ack.drive.speed = speed
    ack.drive.steering_angle = angle[i]
    cmd_vel_pub.publish(ack)
    print("%d. speed = %.1f angle = %.2f"%(i,speed,angle[i]))
    rospy.sleep(dt)
    
    
# racecar_speed_line.main()
    


        




#rospy.spin()

